// #include "sensor_fusion_node.h"
#include <sensor_fusion/sensor_fusion_node.h>

SensorFusionNode::SensorFusionNode(tf2_ros::Buffer* tf_buffer):tfBuffer_(tf_buffer){
    baseFrameId_ = "base_link";
    scanRawConfig_.base_config.topic_name = "/scan_raw";
    scanRawConfig_.base_config.index = 0;

    sensorsFusionPtr_ = boost::make_shared<SensorsFusion>(1,0);

    // 订阅传感器信息
    scanRawSub_ = nh_.subscribe(scanRawConfig_.base_config.topic_name, 10, &SensorFusionNode::ScanRawCallback, this);
    odomSub_ = nh_.subscribe("/odom", 10, &SensorFusionNode::OdomRawCallback, this);
    imuSub_ = nh_.subscribe("/imu", 10, &SensorFusionNode::ImuRawCallback, this);
    fusionCloudPub_ = nh_.advertise<sensor_msgs::PointCloud2>("/fusion_cloud", 10);

    Loop();
}


// 雷达回调函数
void SensorFusionNode::ScanRawCallback(const sensor_msgs::LaserScan::Ptr& msg){
    scanRawConfig_.base_config.frame_id = msg->header.frame_id;
    // 获取静态坐标变换
    if(!scanRawConfig_.base_config.has_transform){
        try{
            geometry_msgs::TransformStamped transform = tfBuffer_->lookupTransform(
                baseFrameId_, scanRawConfig_.base_config.frame_id, msg->header.stamp, ros::Duration(0.2));
            scanRawConfig_.base_config.transform_matrix = msg_convert::Geometry2Affine(transform);
            scanRawConfig_.base_config.has_transform = true;
        }catch(tf2::TransformException &ex){
            ROS_ERROR("%s", ex.what());
            return;
        }
        catch(std::exception &ex){
            ROS_ERROR("%s", ex.what());
            return;
        }
    }

    sensorsFusionPtr_->addScan2DData(scanRawConfig_, msg);
}

// 里程计回调函数
void SensorFusionNode::OdomRawCallback(const nav_msgs::Odometry::Ptr& msg){
    sensorsFusionPtr_->addOdomData(msg);
}

// IMU回调函数
void SensorFusionNode::ImuRawCallback(const sensor_msgs::Imu::Ptr& msg){
    sensorsFusionPtr_->addIMUData(msg);
}

// 循环函数
void SensorFusionNode::Loop(){
    ros::Rate loop_rate(15);        // 15Hz
    while(!ros::isShuttingDown()){
        loop_rate.sleep();
        ros::spinOnce();
        // 获取融合后的点云
        sensor_data::PointCloud2Ptr cloud = sensorsFusionPtr_->getFusionData();
        fusionCloudPub_.publish(*cloud);
    }
}


int main(int argc, char** argv){
    FLAGS_alsologtostderr = true;           // 日志同时输出到stderr
    FLAGS_max_log_size = 1;                 // 单个日志文件大小上限（MB）, 如果设置为0将默认为1
	FLAGS_stop_logging_if_full_disk = true; // 当磁盘写满时，停止输出
    FLAGS_minloglevel = google::WARNING;    // 只记录大于{}等级的日志
    FLAGS_log_dir = "/home/xxty/user_config/logs";
    google::InitGoogleLogging(argv[0]);

    ros::init(argc, argv, "sensor_fusion_node");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    ROS_INFO("sensor_fusion_node start...");

    tf2_ros::Buffer tfBuffer(::ros::Duration(10));
    tf2_ros::TransformListener tfListener(tfBuffer);
    SensorFusionNode node(&tfBuffer);
    ros::spin();

    google::ShutdownGoogleLogging();

    return 0;
}